#include "rclcpp/rclcpp.hpp"
#include "agv_pkg/task_server.hpp"
#include "agv_util/point_util.hpp"

namespace agv_pkg
{
    TaskServer::TaskServer(const rclcpp::NodeOptions &options)
        : nav2_util::LifecycleNode("task_server", "", options)
    {
        RCLCPP_INFO(get_logger(), "TaskServer 节点已创建");
    }
    TaskServer::~TaskServer()
    {
    }
    nav2_util::CallbackReturn TaskServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
    {
        RCLCPP_INFO(get_logger(), "on_configure");
        task_request_service_ = this->create_service<nav_msg_pkg::srv::SendTask>("send_task", std::bind(&TaskServer::task_request_callback, this, std::placeholders::_1, std::placeholders::_2));
        task_cancel_service_ = this->create_service<nav_msg_pkg::srv::CancelTask>("cancel_task", std::bind(&TaskServer::cancel_task_callback, this, std::placeholders::_1, std::placeholders::_2));
        navigation_pose_pub_ = this->create_publisher<Waypoint>("/nav_server/nav_point", 10);
        navigation_feedback_sub_ = this->create_subscription<nav_msg_pkg::msg::NavFeedback>(
            "/nav_server/nav_feedback", 10,
            std::bind(&TaskServer::navigation_feedback_callback, this, std::placeholders::_1));
        navigation_result_sub_ = this->create_subscription<std_msgs::msg::String>(
            "/nav_server/nav_result", 10, [this](const std_msgs::msg::String::SharedPtr msg)
            { this->navigation_result_callback(msg); });
        task_timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&TaskServer::task_timer_callback, this));
        navigation_cancel_pub_ = this->create_publisher<std_msgs::msg::String>("/nav_server/nav_cancel", 1);
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
    {
        RCLCPP_INFO(get_logger(), "on_activate");
        createBond();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
    {
        RCLCPP_INFO(get_logger(), "on_deactivate");
        destroyBond();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
    {
        RCLCPP_INFO(get_logger(), "on_cleanup");
        task_request_service_.reset();
        task_cancel_service_.reset();
        return nav2_util::CallbackReturn::SUCCESS;
    }
    nav2_util::CallbackReturn TaskServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
    {
        RCLCPP_INFO(get_logger(), "on_shutdown");
        return nav2_util::CallbackReturn::SUCCESS;
    }
    void TaskServer::task_request_callback(const std::shared_ptr<nav_msg_pkg::srv::SendTask::Request> request,
                                           std::shared_ptr<nav_msg_pkg::srv::SendTask::Response> response)
    {
        if (!task_queue_.empty())
        { // 当任务队列不为空时，则不处理新的任务
            RCLCPP_INFO(get_logger(), "当前设备已有任务，暂不接收新任务.");
            response->response.code = 1002;
            response->response.message = "当前设备已有任务，暂不接收新任务.";
            return;
        }
        // 打印请求
        auto tasks = request->tasks;
        RCLCPP_INFO(get_logger(), "开始接收任务");
        for (auto task : tasks)
        {
            RCLCPP_INFO(get_logger(), "Task: task_type=%s,end_point_code=%s", task.task_type.c_str(), task.end_point_code.c_str());
            geometry_msgs::msg::PoseStamped poseStamped = agv_util::point_to_pose(task.end_point_code);
            RCLCPP_INFO(get_logger(), "点位转坐标: x=%f,y=%f,z=%f",
                        poseStamped.pose.position.x,
                        poseStamped.pose.position.y,
                        poseStamped.pose.position.z);
            if (poseStamped.header.frame_id != "map")
            {
                RCLCPP_ERROR(get_logger(), "坐标转换失败");
                response->response.code = 1001;
                response->response.message = "坐标转换失败 task:end_point_code=%s", task.end_point_code;
                return;
            }
        }
        RCLCPP_INFO(get_logger(), "校验通过，加入任务队列");
        // 通过校验，添加到任务队列
        for (auto task : tasks)
        {
            task_queue_.push(task);
        }
        response->response.code = 0;
        response->response.message = "success";
    }
    void TaskServer::cancel_task_callback(const std::shared_ptr<nav_msg_pkg::srv::CancelTask::Request> request,
                                          std::shared_ptr<nav_msg_pkg::srv::CancelTask::Response> response)
    {
        std_msgs::msg::String task_id;
        task_id.data = request->id;
        RCLCPP_INFO(get_logger(), "cancel_task_callback task_id=%s", task_id.data.c_str());
        // 清空任务队列
        while (! task_queue_.empty())
        {
            task_queue_.pop();//移除队头元素
        }
        
        navigation_cancel_pub_->publish(task_id);
        // 设置响应
        response->response.code = 0;
        response->response.message = "success";
        RCLCPP_INFO(get_logger(), "reponse: code=%d, message= %s",
                    response->response.code,
                    response->response.message.c_str());
    }

    void TaskServer::handle_task_request(const nav_msg_pkg::msg::Task &task,
                                         geometry_msgs::msg::PoseStamped &goal)
    {
        Waypoint point;
        point.name = task.end_point_code;
        point.type = task.task_type;
        point.pose = goal.pose;
        navigation_pose_pub_->publish(point);
        is_task_running_ = true;
        RCLCPP_INFO(get_logger(), "执行任务: name=%s, type=%s, x=%f, y=%f, w=%f",
                    point.name.c_str(),
                    point.type.c_str(),
                    point.pose.position.x,
                    point.pose.position.y,
                    point.pose.orientation.w);
    }
    void TaskServer::handle_task_cancel()
    {
    }

    void TaskServer::navigation_feedback_callback(const nav_msg_pkg::msg::NavFeedback::SharedPtr feedback)
    {
        RCLCPP_INFO(get_logger(), "feedback_callback %f", feedback->distance_remaining);
    }
    void TaskServer::navigation_result_callback(const std_msgs::msg::String::SharedPtr &result)
    {
        RCLCPP_INFO(get_logger(), "导航结果 %s", result->data.c_str());
        is_task_running_ = false;
        // 上报设备空闲状态
        if (result->data == "SUCCEEDED")
        {
            RCLCPP_INFO(get_logger(), "%s", result->data.c_str());
        }
        else if (result->data == "ABORTED")
        {
        }
        else if (result->data == "CANCELED")
        {
        }
        else if (result->data == "UNKNOWN")
        {
            RCLCPP_ERROR(get_logger(), "Goal failed with code: %s", result->data.c_str());
        }
    }
    void TaskServer::task_timer_callback()
    {
        if (!task_queue_.empty() && is_task_running_ == false)
        {
            nav_msg_pkg::msg::Task task = task_queue_.front();
            task_queue_.pop();
            auto goal = agv_util::point_to_pose(task.end_point_code);
            handle_task_request(task, goal);
        }
    }
} // namespace agv_pkg
